/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package mygame.procedural.ik;

import com.jme3.collision.CollisionResult;
import com.jme3.collision.CollisionResults;
import com.jme3.material.Material;
import com.jme3.math.FastMath;
import com.jme3.math.Ray;
import com.jme3.math.Spline;
import com.jme3.math.Vector3f;
import com.jme3.renderer.RenderManager;
import com.jme3.renderer.ViewPort;
import com.jme3.renderer.queue.RenderQueue;
import com.jme3.scene.Geometry;
import com.jme3.scene.Spatial;
import com.jme3.scene.control.AbstractControl;
import com.jme3.scene.shape.Curve;

/**
 *
 * @author Paulo
 */
public class LegControl extends AbstractControl {

    public Vector3f getRest() {
        return rest;
    }

    public void setRest(Vector3f rest) {
        this.rest = rest;
        this.controlStart.set(rest.add(0, -1, 0));
        this.controlEnd.set(rest.add(0, -1, 0));
    }

    public Spatial getGround() {
        return ground;
    }

    public void setGround(Spatial ground) {
        this.ground = ground;
    }
    
    public Vector3f getFootOffset() {
        return this.footOffset.clone();
    }
    
    public void setFootOffset(Vector3f offset) {
        this.footOffset = offset.clone();
    }
    
    public static enum State {
        Stance, StartSwing, Swing
    }
    
    private Spatial ground;
    
    private LegControl.State state = State.Stance;
    
    private Vector3f rest = new Vector3f();
    private Vector3f footPos = new Vector3f();
    private Vector3f footOffset = new Vector3f();
    
    private Vector3f velocity = new Vector3f();
    
    private Vector3f groundLastStep = new Vector3f();
    private Vector3f groundNextStep = new Vector3f();
    
    private Vector3f controlStart = new Vector3f();
    private Vector3f lastStep = new Vector3f();
    private Vector3f nextStep = new Vector3f();
    private Vector3f controlEnd = new Vector3f();
    
    private Vector3f points[] = new Vector3f[] {
        controlStart, lastStep, nextStep, controlEnd
    };
    
    private float tension = 0.2f;
    private boolean debugMode = false;
    private Geometry curve;
    
    public LegControl() {
        this.ground = null;
    }
    
    public void debug(Material material) {
        debugMode = true;
        curve = new Geometry("curve");
        curve.setMaterial(material);
        curve.setShadowMode(RenderQueue.ShadowMode.Off);
        spatial.getParent().attachChild(curve);
        updateDebug();
    }
    
    private void updateDebug() {
        if (debugMode) {
            Spline spl = new Spline(Spline.SplineType.CatmullRom, points, tension, false);
            Curve c = new Curve(spl, 16);
            curve.setMesh(c);
        }
    }
    
    public void setVelocity(Vector3f velocity) {
        this.velocity = velocity;
    }
    
    private float getStepLength() {
        if (spatial != null) {
            return spatial.getUserData("stepLength");
        } else {
            return 0.0f;
        }
    }
    
    private float getStepHeight() {
        if (spatial != null) {
            return spatial.getUserData("stepHeight");
        } else {
            return 0.0f;
        }
    }
    
    public void stance() {
        if (state == State.Swing) {
            state = State.Stance;
            setLocalLastStep();
        }
    }
    
    private float time = 0;
    public void swing() {
        if (state == State.Stance) {
            state = State.StartSwing;
        }
    }
    
    private Vector3f localVelocity() {
        return spatial.getParent().worldToLocal(velocity.add(spatial.getParent().getWorldTranslation()), null);
    }
    
    private Vector3f getLocalLastStep() {
        lastStep.set(convertCoords(getGround(), spatial.getParent(), groundLastStep));
        return lastStep;
    }
    
    private void setLocalLastStep() {
        groundLastStep.set(convertCoords(spatial.getParent(), getGround(), footPos));
    }
    
    private Vector3f getLocalNextStep() {
        nextStep.set(convertCoords(getGround(), spatial.getParent(), groundNextStep));
        return nextStep;
    }
    
    private void setLocalNextStep() {
        groundNextStep.set(convertCoords(spatial.getParent(), getGround(), nextStep));
    }
    
    private Vector3f dynamicInterp(float t, Vector3f[] points, Vector3f store) {
        if (t > 0.99f) {
            t = 0.99f;
        } else if (t < 0.0f) {
            t = 0.0f;
        }
        
        float cpT = t * (points.length - 3);
        int cp = (int) cpT;
        cpT -= cp;
        cp %= points.length;
        
        return FastMath.interpolateCatmullRom(cpT, tension, points[cp], 
                points[cp + 1], points[cp + 2], points[cp + 3], store);
    }

    private Vector3f convertCoords(Spatial from, Spatial to, Vector3f vector) {
        return to.worldToLocal(from.localToWorld(vector, null), null);
    }
    
    private float stepTime() {
        return getStepLength() / Math.max(velocity.length(), 0.8f);
    }
    
    private void calcPoints() {
        getLocalLastStep();
        nextStep.set(collisionPoint(getRest().add(localVelocity().mult(1.5f*stepTime()))));
        setLocalNextStep();
    }
    
    private Vector3f collisionPoint(Vector3f point) {
        Vector3f origin = point.add(0, getStepHeight(), 0);
        origin = spatial.getParent().localToWorld(origin, null);
        Ray ray = new Ray(origin, Vector3f.UNIT_Y.negate());
        ray.setLimit(2 * getStepHeight());
        
        CollisionResults validResults = new CollisionResults();
        CollisionResults results = new CollisionResults();
        getGround().collideWith(ray, results);
        for (CollisionResult r : results) {
            if (r.getDistance() < ray.getLimit() && !r.getGeometry().getName().equals("Cube1")) {
                validResults.addCollision(r);
            }
        }
        CollisionResult result = validResults.getClosestCollision();
        
        if (result != null) {
            return spatial.getParent().worldToLocal(result.getContactPoint(), null);
        } else {
            return point;
        }
    }
    
    @Override
    protected void controlUpdate(float tpf) {
        updateDebug();
        
        switch (state) {
            case StartSwing:
                time = 0;
                calcPoints();
                state = State.Swing;
                break;
            case Swing:
                time += tpf;
                
                getLocalLastStep();
                getLocalNextStep();
                
                dynamicInterp(time / stepTime(), points, footPos);
                
                if (time > stepTime()) {
                    stance();
                }
                break;
            case Stance:
                getLocalLastStep();
                getLocalNextStep();
                
                dynamicInterp(0, points, footPos);
                /*float dist = rest.distance(lastStep);
                if (dist > 3 * getStepLength()) {
                    swing();
                }*/
                break;
        }
        
        spatial.setLocalTranslation(footPos.add(footOffset));
    }

    @Override
    protected void controlRender(RenderManager rm, ViewPort vp) {
    }
}
